//
//  Quaternion.h
//  TrackingSim
//
//  Created by Garrett Manka on 11/26/11.
//  Copyright (c) 2011. All rights reserved.
//

#ifndef TrackingSim_Quaternion_h
#define TrackingSim_Quaternion_h

#include <cmath>
#include <math/Vector.h>
#include <math/Matrix4x4.h>

class Quaternion
{
public:
    
    Quaternion();
    Quaternion( double _x, double _y, double _z, double _w );
    Quaternion( Sim::Vector _v, double _w );
    
    inline const double x() const{ return m_x; }
    inline void x( double _x ) { m_x = _x; }
    
    inline const double y() const { return m_y; }
    inline void y( double _y ) { m_y = _y; }
    
    inline const double z() const { return m_z; }
    inline void z( double _z ) { m_z = _z; }
    
    inline const double w() const { return m_w; }
    inline void w( double _w ) { m_w = _w; }
    
    inline const Sim::Vector v() const { return Sim::Vector( m_x, m_y, m_z ); }
    inline void v( Sim::Vector _v ) { m_x = _v.x(); m_y = _v.y(); m_z = _v.z(); }
    
    Quaternion unit() const;
    
    Quaternion conjugate() const;
    
    const Matrix4x4 getMatrix4x4() const;
    
    void getAxisAngle(Sim::Vector& axis, double& angle) const;
    
private:
    
    double m_x;
    double m_y;
    double m_z;
    double m_w;
    
};


inline Quaternion Quaternion::unit() const
{
    double recip_mag = 1.0 / sqrt( m_x*m_x + m_y*m_y + m_z*m_z + m_w*m_w );
    
    return Quaternion( m_x * recip_mag, m_y * recip_mag, m_z * recip_mag, m_w * recip_mag );
}

inline Quaternion Quaternion::conjugate() const
{
    return Quaternion( -m_x, -m_y, -m_z, m_w );
}

Quaternion operator*(const Quaternion& a, const Quaternion& b);

void rotateVector( Sim::Vector& vector, const Sim::Vector& axis, double theta );

void rotateVector( Sim::Vector& vector, const Quaternion& q );

Quaternion quatFromAxisAngle( const Sim::Vector& axis, const double angle );

Quaternion quatFromMatrix4x4( const Matrix4x4& m );

#endif
